package org.maxgroundstation.groundstation.view.overlay;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Polygon;
import java.awt.Shape;
import java.awt.Stroke;
import java.awt.geom.AffineTransform;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Rectangle2D;
import java.awt.geom.RoundRectangle2D;
import java.util.Map;

import org.maxgroundstation.groundstation.GroundStation;
import org.maxgroundstation.groundstation.comm.service.geometry.GeometryService;

/**
 * An overlay for displaying rover geometry.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class GeometryOverlay implements Overlay {

    // Static members.
    private static final Color FG_COLOR = Color.GREEN;
    private static final Color BG_COLOR = new Color(0, 0, 0, 127);
    private static final Stroke FG_STROKE = new BasicStroke(2F);
    private static final Stroke BG_STROKE = new BasicStroke(4F);
    
    // Data members.
    private GroundStation gs;
    private boolean display;
    
    /**
     * Constructor
     * @param gs the ground station.
     */
    public GeometryOverlay(GroundStation gs) {
        // Initialize data members.
        this.gs = gs;
        display = true;
    }
    
    @Override
    public boolean canDisplay() {
        return gs.getServiceComm().hasService(GeometryService.SERVICE_ID);
    }

    @Override
    public boolean getDisplay() {
        return display;
    }

    @Override
    public String getName() {
        return "Rover Geometry";
    }
    
    /**
     * Gets the rover geometry service .
     * @return geometry service or null if none.
     */
    private GeometryService getGeometryService() {
        return (GeometryService) gs.getServiceComm().getService(GeometryService.SERVICE_ID);
    }

    @Override
    public void paintOverlay(Graphics g, int panelWidth, int panelHeight) {
        Graphics2D g2d = (Graphics2D) g;
        AffineTransform saveTransform = g2d.getTransform();
        Stroke saveStroke = g2d.getStroke();
        
        int centerX = 100;
        int centerY = panelHeight - 100;
        
        Map<String, Float> geometryMap = getGeometryService().getGeometryMap();
        if (geometryMap != null) {
         
            // Draw chassis.
            float chassisWidth = 50F;
            float chassisHeight = 100F;
            float chassisX = centerX - (chassisWidth / 2F);
            float chassisY = centerY - (chassisHeight / 2F);
            drawShape(g2d, new Rectangle2D.Float(chassisX, chassisY, chassisWidth, chassisHeight));
        
            float wheelWidth = 25F;
            float wheelHeight = 37.5F;
            
            // Draw front wheels.
            float frontAngle = 0F;
            if (geometryMap.containsKey("front_angle")) {
                frontAngle = geometryMap.get("front_angle");
            }
     
            // Front left wheel.
            g2d.rotate(frontAngle, centerX - (chassisWidth / 2F), centerY - (chassisHeight / 2F));
            float flWheelX = centerX - (chassisWidth / 2F) - (wheelWidth / 2F);
            float flWheelY = centerY - (chassisHeight / 2F) - (wheelHeight / 2F);
            drawShape(g2d, new RoundRectangle2D.Float(flWheelX, flWheelY, wheelWidth, wheelHeight, 
                    (wheelWidth / 2F), (wheelWidth / 2F)));
            g2d.setTransform(saveTransform);
            
            // Front right wheel.
            g2d.rotate(frontAngle, centerX + (chassisWidth / 2F), centerY - (chassisHeight / 2F));
            float frWheelX = centerX + (chassisWidth / 2F) - (wheelWidth / 2F);
            float frWheelY = centerY - (chassisHeight / 2F) - (wheelHeight / 2F);
            drawShape(g2d, new RoundRectangle2D.Float(frWheelX, frWheelY, wheelWidth, wheelHeight, 
                    (wheelWidth / 2F), (wheelWidth / 2F)));
            g2d.setTransform(saveTransform);
            
            // Draw rear wheels.
            float rearAngle = 0F;
            if (geometryMap.containsKey("rear_angle")) {
                rearAngle = geometryMap.get("rear_angle");
            }
            
            // Rear left wheel.
            g2d.rotate(rearAngle, centerX - (chassisWidth / 2F), centerY + (chassisHeight / 2F));
            float rlWheelX = centerX - (chassisWidth / 2F) - (wheelWidth / 2F);
            float rlWheelY = centerY + (chassisHeight / 2F) - (wheelHeight / 2F);
            drawShape(g2d, new RoundRectangle2D.Float(rlWheelX, rlWheelY, wheelWidth, wheelHeight, 
                    (wheelWidth / 2F), (wheelWidth / 2F)));
            g2d.setTransform(saveTransform);
            
            // Rear right wheel.
            g2d.rotate(rearAngle, centerX + (chassisWidth / 2F), centerY + (chassisHeight / 2F));
            float rrWheelX = centerX + (chassisWidth / 2F) - (wheelWidth / 2F);
            float rrWheelY = centerY + (chassisHeight / 2F) - (wheelHeight / 2F);
            drawShape(g2d, new RoundRectangle2D.Float(rrWheelX, rrWheelY, wheelWidth, wheelHeight, 
                    (wheelWidth / 2F), (wheelWidth / 2F)));
            g2d.setTransform(saveTransform);
            
            // Draw turret top down.
            float panAngle = 0F;
            if (geometryMap.containsKey("pan")) {
                panAngle = geometryMap.get("pan");
            }
            
            g2d.rotate(panAngle, centerX, centerY);
            float turretDiameter = chassisWidth;
            drawShape(g2d, new Ellipse2D.Float(centerX - (turretDiameter / 2F), 
                    centerY - (turretDiameter / 2F), turretDiameter, turretDiameter));
            drawShape(g2d, new Rectangle2D.Float(centerX - (turretDiameter / 2F), centerY - 2.5F, 
                    turretDiameter, 5F));
            drawShape(g2d, new Rectangle2D.Float(centerX - 15F, centerY - 7.5F, 10F, 5F));
            drawShape(g2d, new Rectangle2D.Float(centerX + 5F, centerY - 7.5F, 10F, 5F));
            g2d.setTransform(saveTransform);
            
            // Draw turret side.
            float tiltAngle = 0F;
            if (geometryMap.containsKey("tilt")) {
                tiltAngle = geometryMap.get("tilt");
            }
            
            float centerTX = centerX + 100F;
            float centerTY = centerY - 50F;
            drawShape(g2d, new Rectangle2D.Float(centerTX - 25F, centerTY + 32.5F, 50F, 5F));
            drawShape(g2d, new Rectangle2D.Float(centerTX - 7.5F, centerTY, 5F, 32.5F));
            g2d.rotate(-1F * tiltAngle, centerTX, centerTY);
            drawShape(g2d, new Rectangle2D.Float(centerTX - 2.5F, centerTY - 16.25F, 5F, 32.5F));
            drawShape(g2d, new Rectangle2D.Float(centerTX + 2.5F, centerTY - 5F, 5F, 10F));
            g2d.setTransform(saveTransform);
            
            // Draw motor power.
            float motorPower = 0F;
            if (geometryMap.containsKey("motor")) {
                motorPower = geometryMap.get("motor");
            }
            
            float centerMX = centerX + 100F;
            float centerMY = centerY + 50F;
            Polygon topTriangle = new Polygon(new int[] {(int) centerMX, (int) centerMX + 37, (int) centerMX}, 
                    new int[] {(int) centerMY - 37, (int) centerMY - 37, (int) centerMY}, 3);
            drawShape(g2d, topTriangle);
            Polygon bottomTriangle = new Polygon(new int[] {(int) centerMX, (int) centerMX - 37, (int) centerMX}, 
                    new int[] {(int) centerMY, (int) centerMY + 37, (int) centerMY + 37}, 3);
            drawShape(g2d, bottomTriangle);
            int fillNum = (int) Math.round(motorPower * 37D);
            int fillX = (int) centerMX + fillNum;
            int fillY = (int) centerMY - fillNum;
            Polygon fillTriangle = new Polygon(new int[] {(int) centerMX, fillX, (int) centerMX}, 
                    new int[] { fillY, fillY, (int) centerMY}, 3);
            fillShape(g2d, fillTriangle);
        }
        
        // Reset original stroke.
        g2d.setStroke(saveStroke);
    }
    
    /**
     * Draw a shape with a background outline.
     * @param g2d the graphics context.
     * @param shape the shape to draw.
     */
    private void drawShape(Graphics2D g2d, Shape shape) {
        g2d.setStroke(BG_STROKE);
        g2d.setColor(BG_COLOR);
        g2d.draw(shape);
        g2d.setStroke(FG_STROKE);
        g2d.setColor(FG_COLOR);
        g2d.draw(shape);
    }
    
    /**
     * Fill a shape with a background outline.
     * @param g2d the graphics context.
     * @param shape the shape to draw.
     */
    private void fillShape(Graphics2D g2d, Shape shape) {
        g2d.setStroke(BG_STROKE);
        g2d.setColor(BG_COLOR);
        g2d.fill(shape);
        g2d.setStroke(FG_STROKE);
        g2d.setColor(FG_COLOR);
        g2d.fill(shape);
    }

    @Override
    public void setDisplay(boolean displayOverlay) {
        this.display = displayOverlay;
    }
}